#include "Camera.h"
#include "SceneNode.h"

namespace Nezha
{

	FrustumPlanes::FrustumPlanes()
		:mCam(NULL)
	{

	}

	FrustumPlanes::~FrustumPlanes()
	{

	}

	void FrustumPlanes::setCamera(const Camera* cam)
	{
		if(mCam != cam)
		{
			mCam = cam;

			if(mCam)
			{
				const Matrix4x4& pv = mCam->getProjectionViewMat();

				Vector4 v, vl, vr;

				// left <1, 0, 0, 1>
				v = pv.getRow(3, vr) + pv.getRow(0, vl);
				vector2plane(v, mPlanes[FP_LEFT]);

				// right <-1, 0, 0, 1>
				v = pv.getRow(3, vr) - pv.getRow(0, vl);
				vector2plane(v, mPlanes[FP_RIGHT]);

				// top <0, -1, 0, 1>
				v = pv.getRow(3, vr) - pv.getRow(1, vl);
				vector2plane(v, mPlanes[FP_TOP]);

				// bottom <0, 1, 0, 1>
				v = pv.getRow(3, vr) + pv.getRow(1, vl);
				vector2plane(v, mPlanes[FP_BOTTOM]);

				if(Camera::GetDepthType() == DT_OGL)
				{
					// near <0, 0, 1, 1>
					v = pv.getRow(3, vr) + pv.getRow(2, vl);
					vector2plane(v, mPlanes[FP_NEAR]);
				}
				else
				{
					// near <0, 0, 1, 0>
					vector2plane(pv.getRow(3, v), mPlanes[FP_NEAR]);
				}

				// far <0, 0, -1, 1>
				v = pv.getRow(3, vr) - pv.getRow(2, vl);
				vector2plane(v, mPlanes[FP_FAR]);

				for(int i = 0; i < FP_NUM; i++)
				{
					float len = mPlanes[i].mNormal.normalize();

					if(len > NZ_TOLERANCE)
					{
						mPlanes[i].mConstant /= len;
					}
				}
			}
		}
	}

	const Plane& FrustumPlanes::getPlane(FrustumPlane fp) const
	{
		return mPlanes[fp];
	}


	// for default, we're a opengl es 2.0 rendering engine.
	DepthType Camera::msDepthType = DT_OGL;

	void Camera::_SetDepthType(DepthType dt)
	{
		msDepthType = dt;
	}

	DepthType Camera::GetDepthType()
	{
		return msDepthType;
	}

	Camera::Camera(const _string& name)
		:mName(name)
		,mPerspective(true)
		,mOrientationDirty(true)
		,mCullingMask(NEZHA_CULLING_MASK_ALL)
	{
		//setFrame(Vector3::ZERO, -Vector3::UNIT_Z, Vector3::UNIT_Y, Vector3::UNIT_X);
		mQuat = Quaternion::IDENTITY;
		mPosition = Vector3::ZERO;
		_updateViewMatrix();
		setFrustum(60.0f, 1.0f, 0.1f, 300.0f);
	}

	Camera::Camera(const _string& name, bool perspective)
		:mName(name)
		,mPerspective(perspective)
		,mOrientationDirty(true)
		,mCullingMask(NEZHA_CULLING_MASK_ALL)
	{
		//setFrame(Vector3::ZERO, -Vector3::UNIT_Z, Vector3::UNIT_Y, Vector3::UNIT_X);
		mQuat = Quaternion::IDENTITY;
		mPosition = Vector3::ZERO;
		_updateViewMatrix();
		setFrustum(60.0f, 1.0f, 0.1f, 300.0f);
	}

	Camera::~Camera()
	{

	}

	void Camera::setPosition(const Vector3& pos)
	{
		mPosition = pos;
		_updateViewMatrix();
	}

	void Camera::setDirection(const Vector3& dir)
	{
		//mDirection = dir;
		//updateAxes();
		//return;

		if(dir == Vector3::ZERO)
		{
			return;
		}

		Vector3 negZDir = dir;
		Vector3::FastNormalize(negZDir);

		Vector3 d = mQuat * (-Vector3::UNIT_Z);
		Quaternion quat;

		if((d + negZDir).sqrLength() < 0.00005f)
		{
			// a 180 degree turn
			Vector3 u = mQuat * Vector3::UNIT_Y;
			quat.fromAngleAxis(NZ_PI, u);
		}
		else
		{
			quat = Quaternion::makeRotate(d, negZDir);
		}

		mQuat = quat * mQuat;

		_updateViewMatrix();
	}

	void Camera::setLookAt(const Vector3& target)
	{
		setDirection(target - mPosition);
	}

	Vector3 Camera::getDirection() const
	{
		// camera point to negtive-z.
		return mQuat * (-Vector3::UNIT_Z);
	}

	Vector3 Camera::getUp() const
	{
		return mQuat * Vector3::UNIT_Y;
	}

	Vector3 Camera::getRight() const
	{
		return mQuat * Vector3::UNIT_X;
	}

	void Camera::setOrientation(const Quaternion& quat)
	{
		mQuat = quat;

		_updateViewMatrix();
	}

	void Camera::setFrustum(const FrustumDef& f)
	{
		memcpy(&mFrustumDef, &f, sizeof(f));

		_updateProjectionMatrix();
	}

	void Camera::setFrustum(float fovY, float aspect, float _near, float _far)
	{
		float halfAngle = 0.5f * fovY * NZ_DEGTORAD;

		mFrustumDef.mTop = _near * Tan(halfAngle);
		mFrustumDef.mBottom = -mFrustumDef.mTop;
		mFrustumDef.mRight = aspect * mFrustumDef.mTop;
		mFrustumDef.mLeft = -mFrustumDef.mRight;
		mFrustumDef.mNear = _near;
		mFrustumDef.mFar = _far;

		_updateProjectionMatrix();
	}

	void Camera::rotate(const Vector3& axis, float angle)
	{
		Quaternion quat(axis, angle);
		mQuat = quat * mQuat;
		
		_updateViewMatrix();
	}

	void Camera::getFrustumPlaneViewSpace(FrustumPlane fp, Plane& pl)
	{
		// extract view's space frustum planes from project matrix.
		// our project matrix M, M3 = <0, 0, 1(or -1), 0>

		pl.mConstant = 0;

		switch(fp)
		{
		case FP_LEFT:
			// <1, 0, 0, 1> = M0 + M3, M30 = 0, M01 = M31 = 0
			pl.mNormal.set(mProjectionMat[0][0], 0, mProjectionMat[0][2] + mProjectionMat[3][2]);
			break;
		case FP_RIGHT:
			// <-1, 0, 0, 1> = M3 - M0, M30 = 0, M01 = M31 = 0
			pl.mNormal.set(0 - mProjectionMat[0][0], 0, mProjectionMat[3][2] - mProjectionMat[0][2]);
			break;
		case FP_TOP:
			// <0, -1, 0, 1> = M3 - M1
			pl.mNormal.set(0, 0 - mProjectionMat[1][1], mProjectionMat[3][2] - mProjectionMat[1][2]);
			break;
		case FP_BOTTOM:
			// <0, 1, 0, 1> = M3 + M1
			pl.mNormal.set(0, 0 + mProjectionMat[1][1], mProjectionMat[3][2] + mProjectionMat[1][2]);
			break;
		case FP_NEAR:
			{
				if(msDepthType == DT_OGL)
				{
					// <0, 0, 1, 1> = M3 + M2
					pl.mNormal.set(0, 0, mProjectionMat[3][2] + mProjectionMat[2][2]);
					pl.mConstant = mProjectionMat[3][3] + mProjectionMat[2][3];
				}
				else
				{
					// for DX, just M3
					pl.mNormal.set(0, 0, mProjectionMat[3][3]);
				}
			}
			break;
		case FP_FAR:
			// <0, 0, -1, 1> = M3 + M2
			pl.mNormal.set(0, 0, mProjectionMat[3][2] - mProjectionMat[2][2]);
			pl.mConstant = mProjectionMat[3][3] - mProjectionMat[2][3];
			break;
		}
	}

	FrustumPlane Camera::cull(const Vector3& center, float radius, FrustumPlanes& frustumPlanes) const
	{
		frustumPlanes.setCamera(this);

		for(int i = 0; i < FP_NUM; i++)
		{
			const Plane& p = frustumPlanes.getPlane((FrustumPlane)i);

			if(p.distane2(center) < -radius)
			{
				return (FrustumPlane)i;
			}
		}

		return FP_NONE;
	}

	FrustumPlane Camera::cull(const AABB& bound, FrustumPlanes& frustumPlanes) const
	{
		if(bound.notValid())
			return FP_NONE;

		frustumPlanes.setCamera(this);

		Vector3 c = bound.getCenter();
		Vector3 halfSize = bound.getSize() * 0.5f;

		for(int i = 0; i < FP_NUM; i++)
		{
			const Plane& p = frustumPlanes.getPlane((FrustumPlane)i);

			Plane::Side side = p.whichSide(c, halfSize);
			
			if(side == Plane::SIDE_NEGATIVE)
			{
				return (FrustumPlane)i;
			}
		}

		return FP_NONE;
	}

	//void Camera::_updateViewMatrix()
	//{
	//	/*
	//				| R |
	//		M =	| U | [X - P]
	//				| D |
	//	*/

	//	mViewMat.setRow(0, mRight.x, mRight.y, mRight.z, -mPosition.dot(mRight));
	//	mViewMat.setRow(1, mUp.x, mUp.y, mUp.z, -mPosition.dot(mUp));
	//	mViewMat.setRow(2, mDirection.x, mDirection.y, mDirection.z, -mPosition.dot(mDirection));
	//	mViewMat.setRow(3, 0, 0, 0, 1.0f);

	//	mPVMat = mProjectionMat * mViewMat;
	//}

	void Camera::_updateViewMatrix()
	{
		Matrix3x3 rot;
		mQuat.toRotation(rot);

		Matrix3x3 rotT = rot.transpose();
		Vector3 trans = rotT * mPosition;

		mViewMat.setRow(0, rotT[0][0], rotT[0][1], rotT[0][2], -trans.x);
		mViewMat.setRow(1, rotT[1][0], rotT[1][1], rotT[1][2], -trans.y);
		mViewMat.setRow(2, -rotT[2][0], -rotT[2][1], -rotT[2][2], trans.z);
		mViewMat.setRow(3, 0, 0, 0, 1);

		mPVMat = mProjectionMat * mViewMat;
	}

	void Camera::_updateProjectionMatrix()
	{
		/*
			perspective, z[0, 1]
			x1 = n*x / z => (x1 - (l + r) / 2) / ((r - l) / 2) -> [-1, 1] => v1 = (2*n*x / (r - l) - (r+l)/(r-l)*z) / z
			v2 = (2*n*y/(t-b) - (t+b)/(t-b)*z) / z
			z2 = (a*z + b) / z <=> a + b/z, a + b/n = 0 && a + b/f = 1 => b = n*f / (n - f)  a = f / (f-n)
			v3 = f / (f-n) *z +  n*f / (n-f)
			v4 = <0, 0, 1, 0>

			orthographic, z[0, 1] w = 1
			x1 = x => (x - (l+r)/2) / ((r-l)/2) -> [-1, 1] => v1 = (2*x / (r-l) - (r+l) / (r-l))
			v2 = (2*y / (r-l) - (r+l) / (r-l))
			z => (z - n) / (f- n) -> [0, 1] => v3 = 1 / (f-n) - n / (f-n)
			v4 = <0, 0, 0, 1>
		*/
		float n = mFrustumDef.mNear;
		float f = mFrustumDef.mFar;
		float l = mFrustumDef.mLeft;
		float r = mFrustumDef.mRight;
		float t = mFrustumDef.mTop;
		float b = mFrustumDef.mBottom;

		float invDiff_fn = 1.0f / (f - n);
		float invDiff_rl = 1.0f / (r - l);
		float invDiff_tb = 1.0f / (t - b);
		float Sum_fn_Mul_invDiff_fn = (f + n) * invDiff_fn;
		float Sum_rl_Mul_invDiff_rl = (r + l) * invDiff_rl;
		float Sum_tb_Mul_InvDiff_tb = (t + b) * invDiff_tb;

		if(mPerspective)
		{
			if(msDepthType == DT_DX)
			{
				mProjectionMat.setRow(0, 2.0f * n * invDiff_rl, 0, -1.0f * Sum_rl_Mul_invDiff_rl, 0);
				mProjectionMat.setRow(1, 0, 2.0f * n * invDiff_tb, -1.0f * Sum_tb_Mul_InvDiff_tb, 0);
				mProjectionMat.setRow(2, 0, 0, f * invDiff_fn, -1.0f * n * f * invDiff_fn);
				mProjectionMat.setRow(3, 0, 0, 1, 0);
			}
			else
			{
				/*
					z2 = a + b/z => a + b/n = -1 a + b/f = 1 => a = (f + n)/(f-n) b = -2*n*f /(f-n)
				*/
				mProjectionMat.setRow(0, 2.0f * n * invDiff_rl, 0, -1.0f * Sum_rl_Mul_invDiff_rl, 0);
				mProjectionMat.setRow(1, 0, 2.0f * n * invDiff_tb, -1.0f * Sum_tb_Mul_InvDiff_tb, 0);
				mProjectionMat.setRow(2, 0, 0, Sum_fn_Mul_invDiff_fn, -2.0f * n * f * invDiff_fn);
				mProjectionMat.setRow(3, 0, 0, 1.0f, 0);

				// opengl type project matrix
				//mProjectionMat.setRow(0, 2.0f * n * invDiff_rl, 0, Sum_rl_Mul_invDiff_rl, 0);
				//mProjectionMat.setRow(1, 0, 2.0f * n * invDiff_tb, Sum_tb_Mul_InvDiff_tb, 0);
				//mProjectionMat.setRow(2, 0, 0, -1.0f * Sum_fn_Mul_invDiff_fn, -2.0f * n * f * invDiff_fn);
				//mProjectionMat.setRow(3, 0, 0, -1.0f, 0);
			}
		}
		else
		{
			if(msDepthType == DT_DX)
			{
				mProjectionMat.setRow(0, 2.0f * invDiff_rl, 0, 0, -1.0f * Sum_rl_Mul_invDiff_rl);
				mProjectionMat.setRow(1, 0, 2.0f * invDiff_tb, 0, -1.0f * Sum_tb_Mul_InvDiff_tb);
				mProjectionMat.setRow(2, 0, 0, invDiff_fn, -1.0f * n * invDiff_fn);
				mProjectionMat.setRow(3, 0, 0, 0, 1.0f);
			}
			else
			{
				/*
					z2 = (z - (n+f)/2) / ((f-n)/2) -> [-1, 1]
				*/
				mProjectionMat.setRow(0, 2.0f * invDiff_rl, 0, 0, -1.0f * Sum_rl_Mul_invDiff_rl);
				mProjectionMat.setRow(1, 0, 2.0f * invDiff_tb, 0, -1.0f * Sum_tb_Mul_InvDiff_tb);
				mProjectionMat.setRow(2, 0, 0, 2.0f * invDiff_fn, -1.0f * Sum_fn_Mul_invDiff_fn);
				mProjectionMat.setRow(3, 0, 0, 0, 1.0f);
			}
		}

		mPVMat = mProjectionMat * mViewMat;
	}

	void Camera::_setPerspective(bool b)
	{
		if(mPerspective != b)
		{
			mPerspective = b;

			_updateProjectionMatrix();
		}
	}

	void Camera::setCullingMask(u32 cullingMask)
	{
		mCullingMask = cullingMask;
	}
}
